#!/usr/bin/env python
"""
视频捕获与发布节点

该节点负责从指定的IP摄像头（或本地摄像头作为备用）捕获视频流，并以两种方式发布图像：

1.  **连续实时流**: 将摄像头的每一帧都发布到 '/ip_camera/image_raw' 话题。
    这可用于实时监控、调试或在Rviz等工具中显示。

2.  **按需快照**: 订阅 'visual_command' 话题。当收到特定指令时，
    它会将当前最新的一帧图像发布到 'visual_info' 话题。这个机制允许
    其他节点（如视觉识别节点）按需获取单帧图像进行分析，避免了对整个
    视频流进行不必要的处理，从而节省计算资源。
"""
import rospy
import cv2
from sensor_msgs.msg import Image
from std_msgs.msg import String
from cv_bridge import CvBridge, CvBridgeError

class IPCameraNode:
    """
    封装了摄像头视频捕获和ROS图像发布逻辑的类。
    """
    def __init__(self):
        """
        初始化IPCameraNode。
        """
        rospy.init_node('ip_camera_publisher', anonymous=True)
        
        # CvBridge用于在OpenCV图像和ROS图像消息之间进行转换
        self.bridge = CvBridge()
        # 用于存储最新捕获的视频帧
        self.current_frame = None
        
        # --- ROS Publishers & Subscribers ---
        # 发布器：用于连续发布实时视频流
        self.image_pub = rospy.Publisher("/ip_camera/image_raw", Image, queue_size=1)
        # 发布器：用于按需发布单帧图像给视觉识别节点
        self.visual_info_pub = rospy.Publisher("visual_info", Image, queue_size=1)
        
        # 订阅器：接收来自中央控制节点的指令
        rospy.Subscriber("visual_command", String, self.cmd_callback)
        
        # --- 摄像头初始化 ---
        # 尝试连接到指定的IP摄像头URL
        ip_camera_url = "http://192.168.189.69:11311/videofeed?mjpeg=1"
        self.cap = cv2.VideoCapture(ip_camera_url)
        
        # 如果IP摄像头连接失败，则回退到使用系统默认的本地摄像头（通常是索引0）
        if not self.cap.isOpened():
            rospy.logwarn(f"无法打开IP摄像头: {ip_camera_url}。正在尝试使用默认摄像头(0)...")
            self.cap = cv2.VideoCapture(0)
            if not self.cap.isOpened():
                rospy.logerr("无法打开任何摄像头！请检查摄像头连接或URL。")
                return
        
        rospy.loginfo("摄像头节点已成功启动并连接。")
    
    def cmd_callback(self, msg):
        """
        处理来自 'visual_command' 话题的指令。
        当接收到 "true" 指令时，将当前缓存的最新一帧发布出去。
        """
        # 检查指令是否为"true"以及当前是否有可用的帧
        if msg.data.lower() == "true" and self.current_frame is not None:
            try:
                # 将OpenCV图像帧转换为ROS Image消息并发布
                visual_image_msg = self.bridge.cv2_to_imgmsg(self.current_frame, "bgr8")
                self.visual_info_pub.publish(visual_image_msg)
                rospy.loginfo("收到指令，已将当前快照发布到 'visual_info' 话题。")
            except CvBridgeError as e:
                rospy.logerr(f"从OpenCV帧到ROS Image消息的转换失败: {e}")
        else:
            if self.current_frame is None:
                rospy.logwarn("收到指令，但当前没有可用的视频帧。")
    
    def run(self):
        """
        节点的主运行循环。
        以设定的频率持续从摄像头读取视频帧。
        """
        # 设置循环频率为30Hz
        rate = rospy.Rate(30)
        
        while not rospy.is_shutdown() and self.cap.isOpened():
            try:
                # 从摄像头读取一帧
                ret, frame = self.cap.read()
                if ret:
                    # 将读取到的帧保存为当前帧，供回调函数使用
                    self.current_frame = frame
                    
                    # 将帧转换为ROS Image消息，并发布到实时流话题
                    ros_image = self.bridge.cv2_to_imgmsg(frame, "bgr8")
                    self.image_pub.publish(ros_image)
                else:
                    # 使用throttle来避免日志刷屏
                    rospy.logwarn_throttle(5, "无法从摄像头源读取到视频帧。")
            except CvBridgeError as e:
                rospy.logerr_throttle(5, f"图像转换时发生错误: {e}")
            except Exception as e:
                rospy.logerr_throttle(5, f"摄像头读取循环中发生未知错误: {e}")
            
            rate.sleep()
        
        # 节点关闭时，释放摄像头资源
        self.cap.release()
        rospy.loginfo("摄像头资源已成功释放。")

def main():
    """
    主函数，创建并运行IPCameraNode实例。
    """
    try:
        node = IPCameraNode()
        node.run()
    except rospy.ROSInterruptException:
        rospy.loginfo("IP摄像头节点已关闭。")

if __name__ == '__main__':
    main()